#ifndef __LQRCONTROLLER_H
#define __LQRCONTROLLER_H

#include "main.h"
#include <stdio.h>
#include <math.h>


class LQRController
{
public:
    //状态变量实际值
    float current_state[3];
    //采样时间
    float dt;
    // 系统矩阵
    float A_ptr[3][3];
    float *B_ptr;
    // 输出矩阵 (假设只能测量角度和角速度)
    float C_ptr[2][3];
    // LQR反馈增益矩阵 (假设已经计算好)
    float *K_ptr;
    // 状态观测器增益矩阵 (假设已经计算好)
    float L_ptr [3][2];//二维数组指针
    float u;
    LQRController(float *k_buff, float (*l_buff)[3][2], float (*a_buff)[3][3], float *b_buff, float (*c_buff)[2][3], float T);
    ~LQRController() {}
    float lqr_control(float setval[3]);
    // 状态观测器（用来观测无法测量状态）
    void update_observer(void);
};



#endif

/*=================K阵计算方法=================*/
/*
% 定义系统状态空间模型
A = [0 1 0; 0 0 1; 0 0 0];
B = [0; 0; 1];

% 定义权重矩阵 Q 和 R
Q = diag([10, 1, 0.1]); % 状态加权矩阵
R = 1;                  % 控制加权矩阵

% 使用 lqr 函数计算反馈增益矩阵 K
[K, S, E] = lqr(A, B, Q, R);

% 输出反馈增益矩阵 K
disp('反馈增益矩阵 K:');
disp(K);

% 输出闭环系统的特征值
disp('闭环系统的特征值:');
disp(E);
*/

/*=================L阵计算方法=================*/
/*
% 定义系统状态空间模型
A = [0 1 0; 0 0 1; 0 0 0];
C = [1 0 0; 0 1 0];

% 定义观测器的期望极点
p = [-10, -10, -10];

% 使用 place 函数计算观测器增益矩阵 L
L = place(A', C', p);

% 输出观测器增益矩阵 L
disp('观测器增益矩阵 L:');
disp(L);
*/